#include "laserkeyboard.h"
#include "CaliCam.hpp"
#include "constants.h"
#include <vector>
#include <QApplication>
#include <QEvent>
#include <QKeyEvent>
#include <QDebug>
#include <cctype>

using namespace std;

using namespace cv;

const cv::Point LaserKeyboard::square_pts[4]= {cv::Point{12, 180}, cv::Point{1278, 220}, cv::Point{1278, 500}, cv::Point{12, 500}};

bool LaserKeyboard::isInSquareMask(const cv::Point &p) {
    return p.x > square_pts[0].x && p.x < square_pts[1].x && p.y > square_pts[0].y && p.y < square_pts[2].y;
}

LaserKeyboard::LaserKeyboard(QObject *parent, QTextEdit* textEdit) : QThread(parent), textEdit(textEdit)
{
    FileStorage fs(Constants::keyboardLayoutPath, FileStorage::READ);
    this->keyMap = fs["KEYBOARD"].mat();
    this->initKeyBoardMask();
}

void LaserKeyboard::initKeyBoardMask()
{
    KeyBoardMask = Mat::zeros(CAM_HEIGHT, CAM_WIDTH, CV_8U);
    //const cv::Point pts[6] = {cv::Point(1662, 618), cv::Point(1564, 762), cv::Point(256, 750), cv::Point(180, 622), cv::Point(366, 448), cv::Point(1498, 440)};
    //const cv::Point pts[6] = {cv::Point(1576, 804), cv::Point(1664, 660), cv::Point(1498, 488), cv::Point(362, 496), cv::Point(180, 670), cv::Point(250, 804)};
//    const cv::Point pts[9] = {cv::Point(1243, 460), cv::Point(1276, 404), cv::Point(1278, 339), cv::Point(1172, 202), cv::Point(113, 209), cv::Point(12, 314), cv::Point(27, 393), cv::Point(3, 432), cv::Point(19, 489)};
//        fillConvexPoly(KeyBoardMask, pts, 9, Scalar(255), LINE_8);
    fillConvexPoly(KeyBoardMask, square_pts, 4, Scalar(255), LINE_8);

    FileStorage fs(Constants::keyboardLayoutPath, FileStorage::READ);
    keyMap = fs["KEYBOARD"].mat();
    //fillConvexPoly(KeyBoardMask, pts, 6, Scalar(255), LINE_8);
    //imshow("mask", KeyBoardMask);
    //waitKey(0);
}



Point3f LaserKeyboard::getWorldPos(int u, int v)
{
    Mat rotationMat;
    rotationMat.create(3,3,cv::DataType<double>::type);
    cv::Rodrigues(CaliCam::instance()->rvec,rotationMat);
    Point2f uvP(u, v);
    vector<Point2f> points, newPoints;
    points.push_back(uvP);
    cv::Mat uvPoint = cv::Mat::ones(3,1,cv::DataType<double>::type);
#ifdef USE_UNDISTORT
    undistortPoints(points, newPoints, CaliCam::instance()->intrinsics, CaliCam::instance()->distCoeffs, noArray(), CaliCam::instance()->intrinsics);
    uvPoint.at<double>(0,0) = newPoints[0].x;
    uvPoint.at<double>(1,0) = newPoints[0].y;
#else
    uvPoint.at<double>(0,0) = points[0].x;
    uvPoint.at<double>(1,0) = points[0].y;
#endif
    Mat cVec = -rotationMat.inv() * CaliCam::instance()->tvec;
    cv::Mat wcPoint = rotationMat.inv() * (CaliCam::instance()->intrinsics.inv() * uvPoint - CaliCam::instance()->tvec);
    // cout << "uvPoint " << uvPoint << endl;
    //cout << "wcPoint " << wcPoint << " cVec " << cVec << endl;
    Mat onPaperP = cVec - (cVec.at<double>(0, 0)) / (wcPoint.at<double>(0, 0) - cVec.at<double>(0, 0)) * (wcPoint - cVec);
    Point3f realPoint(onPaperP.at<double>(0, 0), onPaperP.at<double>(1, 0), onPaperP.at<double>(2, 0));
#ifdef Find_World
    cout << "Point in image ( " << points[0].x << ", " << points[0].y << ") in real world is ( " << realPoint.x << ", " << realPoint.y << ", " << realPoint.z << " ) " << endl;
#endif
#ifdef CORR_POS
//    realPoint.z -= (0.0332f * realPoint.z + 4.3772f);
    realPoint.z -= (0.2 * realPoint.z + 4.3772f);
#endif
    return realPoint;
}


void onMouseShowValue(int event, int x, int y, int flags, void* param)
{
    static int count = 56;
    Mat *im = reinterpret_cast<Mat*>(param);
    switch (event)
    {

        case CV_EVENT_LBUTTONDOWN:

            int xx = x * 2; int yy = y * 2;
#ifdef STD_LOG
            std::cout<< "Point " << ++count << " at("<<xx<<","<<yy<<") value is " << (int)im->at<uchar>(Point(x, y)) << std::endl;
#endif
            //imshow("testMat", staMat);
            //fprintf(w_fp, "%d %d %d\n", count, x, y);
            break;
    }
    //std::cout << "pressed" << std::endl;
}

cv::Point LaserKeyboard::findLowestPoint(const Contour& cont)
{
    int lowest = 0;
    cv::Point lowP;
    for(Contour::const_iterator it = cont.begin(); it != cont.end(); ++it) {
        if(it->y > lowest) {
            lowest = it->y;
            lowP = *it;
        }
    }
    return lowP;
}




void LaserKeyboard::openImg(Mat &src)
{
    Mat kernel = getStructuringElement(MORPH_CROSS, Size(4, 4));
    morphologyEx(src, src, MORPH_OPEN, kernel, Point(-1, -1), 2);
}

bool LaserKeyboard::isModifyKey(int key)
{
    for(int i = 0; i < 4; ++i)
        if(key == this->modifyKeys[i])
            return 1;
    return 0;
}



Qt::KeyboardModifiers getKeyModifier(int key)
{
    Qt::KeyboardModifiers modifier = Qt::NoModifier;
    switch (key) {
        case Qt::Key_Shift: modifier = Qt::ShiftModifier;
                            break;
        case Qt::Key_Control: modifier = Qt::ControlModifier;
                            break;
        case Qt::Key_Alt: modifier = Qt::AltModifier;
                            break;
        case Qt::Key_Meta: modifier = Qt::MetaModifier;
                            break;
    }
    return modifier;
}


bool ContCmp(const Cont& a, const Cont & b) {
    return a.area > b.area;
}




int LaserKeyboard::handleImg(Mat &src, double fps)
{

    Mat dst;

    cvtColor(src, dst, COLOR_BGR2GRAY);
    GaussianBlur(dst, dst, Size(3, 3), 10, 10);
    threshold(dst, dst, 190, 255, THRESH_TOZERO);
    //openImg(dst);
    Contours contours;

    vector<Vec4i> hierarchy;
    findContours(dst, contours, hierarchy, RETR_LIST, CHAIN_APPROX_NONE);
    vector<Cont> cts;

    for(size_t i = 0; i < contours.size(); ++i) {
        drawContours(src, contours, i, Scalar(i * 0x777 % 156, i * 0x888 % 156, i * 0x999 % 156), 3, LINE_8);
        double area = contourArea(contours[i]);
        if(area > AREA_GATE)
            cts.push_back(Cont(contours[i], area ) );
    }

    sort(cts.begin(), cts.end(), ContCmp);
    bool flag = 0;
    vector<Point> tips;
    for(size_t i = 0; i < cts.size(); ++i) {
        if(cts[i].area < AREA_GATE)
            break;
        flag = 1;
        Moments mu = moments(cts[i].cont);
        int xc = mu.m10 / mu.m00, yc = mu.m01 / mu.m00;
        //cout << "Cont " << i << " Center at ( " << xc << ", " << yc << ") Area is " << cts[i].area << " ";
        Point lowP = findLowestPoint(cts[i].cont);
        tips.push_back(lowP);
#ifdef DEBUG_SHOW
        circle(src, lowP, 10, Scalar(0, 155, 155), 3);
#endif
    }
    if(flag) {

#ifdef STD_LOG
        cout << endl;
#endif
     }
    set<int> tempKeys;
    map<int, clock_t> tempTimes;
    vector<int> text_keys;
    vector<Qt::KeyboardModifiers> modify_keys;
    size_t not_print_key_num = 0;
    bool isResting = 0;
    set<int> allKeys;
    clock_t time1 = clock();
    for(size_t i = 0; i < tips.size(); ++i) {
        Point3f realP = getWorldPos(tips[i].x, tips[i].y);
        int key = this->keyMap.at<int>(Point(ceil(-realP.y), ceil(realP.z)));
        if(key == 0)
            continue;
        allKeys.insert(key);
        if(!isprint(key))
            not_print_key_num++;
#ifdef STD_LOG
        printf(" (%d, %d) world Pos is (%f, %f) \n", tips[i].x, tips[i].y, realP.y, realP.z);
#endif
    }
    if(tips.size() - not_print_key_num > 1)
        isResting = 1;
    if(!isResting) {
        for(set<int>::iterator it = allKeys.begin(); it != allKeys.end(); it++) {
            int key = *it;

            bool shouldPress = 0;
            if(pressedTime.count(key)) {
                clock_t time2 = pressedTime[key];
                double dur_time = (double)(time1 - time2)/CLOCKS_PER_SEC;
                if(dur_time > PRESS_TIME_GATE)
                    shouldPress = 1;
            }
            else shouldPress = 1;
            if(shouldPress && tempTimes.count(key) == 0) {
                if(isModifyKey(key))
                    modify_keys.push_back(getKeyModifier(key));
                else
                    text_keys.push_back(key);
            }
            tempKeys.insert(key);
            tempTimes.insert(make_pair(key, time1));
        }

        Qt::KeyboardModifiers modifyState = Qt::NoModifier;

        for(size_t i = 0; i < modify_keys.size(); ++i)
            modifyState |= modify_keys[i];

        if(text_keys.size() > 0) {
            for(size_t i = 0; i <text_keys.size(); ++i) {
                QString str;
                if(isprint(text_keys[i])) {
                    if((modifyState & Qt::ShiftModifier) && islower(text_keys[i]))
                        text_keys[i] = toupper(text_keys[i]);
                    str = QChar(text_keys[i]);
    #ifdef QDEBUG_LOG
                    qDebug() << "pressed " << str;
    #endif
                }
                QKeyEvent *event = new QKeyEvent(QEvent::KeyPress, text_keys[i] ,modifyState, str, false, text_keys.size());
                QApplication::postEvent(this->textEdit, event);
                emit this->countChanged(1);
            }
        }
        else if(modify_keys.size() > 0) {
            for(size_t i = 0; i < modify_keys.size(); ++i) {
                QKeyEvent *event = new QKeyEvent(QEvent::KeyPress, modify_keys[i] ,modifyState, QString(), false, modify_keys.size());
                QApplication::postEvent(this->textEdit, event);
            }
        }
        for(set<int>::iterator it = pressedKeys.begin(); it != pressedKeys.end();) {
            if(!tempKeys.count(*it)) {
                QString str;
                if(isprint(*it)) {
                    str = QChar(*it);
    #ifdef QDEBUG_LOG
                    clock_t time2 = pressedTime[*it];
                    qDebug() << "release " << str << " use time " << (double)(time1 - time2)/CLOCKS_PER_SEC << " sec";
    #endif
                }
                QKeyEvent *event = new QKeyEvent(QEvent::KeyRelease, *it, modifyState, str, false);
                QApplication::postEvent(this->textEdit, event);
                pressedTime.erase(*it);
                it = pressedKeys.erase(it);

            }
            else it++;
        }
        for(set<int>::iterator it = tempKeys.begin(); it != tempKeys.end(); ++it) {
            pressedKeys.insert(*it);
            pressedTime.insert(*tempTimes.find(*it));
        }
    }
    return flag;





#ifdef DEBUG_SHOW
    if(fps > 1e-8)
        putText(src, std::to_string(fps), Point(50, 50), FONT_HERSHEY_SIMPLEX , 2, Scalar(255, 255, 255));
    namedWindow("dst", WINDOW_NORMAL);
    namedWindow("src", WINDOW_NORMAL);
    setMouseCallback("dst", onMouseShowValue, reinterpret_cast<void*> (&staMat));
    resize(src, staMat, Size(), 0.5, 0.5);
    resize(dst, dst, Size(), 0.5, 0.5);
    moveWindow("dst", 0, 500);
    imshow("src", staMat);
    imshow("dst", dst);
#endif

}

void LaserKeyboard::run()
{
    qDebug() << "Begin to run";
    this->findTapPos(1);
}





void LaserKeyboard::findTapPos(int CamIndex)
{
    VideoCapture cam(CamIndex);
    //cam.set(CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
    //cam.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    //cam.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    //cam.set(CV_CAP_PROP_FPS, 60);

    Mat image;
    int imgCount = 0;
    double fps = 0;
    std::string pathPath = "./chessboard_";
#ifdef STD_LOG
    printf("width = %.2f\n",cam.get(CV_CAP_PROP_FRAME_WIDTH));
    printf("height = %.2f\n",cam.get(CV_CAP_PROP_FRAME_HEIGHT));
#endif
#ifdef QDEBUG_LOG
    qDebug() << "width =" << cam.get(CV_CAP_PROP_FRAME_WIDTH) << " height " <<  cam.get(CV_CAP_PROP_FRAME_HEIGHT);
#endif
    initKeyBoardMask();
    long long frameCount = 0;
    queue<double> time_q;
    while(1) {
        frameCount ++;
        cam >> image;
        //std::cout << "(" << image.cols << " , " << image.rows << ")\n";
        if(image.empty()) {
#ifdef STD_LOG
            std::cout << "empty" << std::endl;
#endif
#ifdef QDEBUG_LOG
            qDebug() << "empty";
#endif
            continue;
        }

        Mat dst;
        image.copyTo(dst, KeyBoardMask);
        handleImg(dst, fps);

        //resize(image, dst, Size(), 0.5, 0.5);
        //imshow("test", dst);
        int key = waitKey(1);
        if(key==27)
            break;
        else if(key == 's') {
            imgCount++;
            imwrite(pathPath + std::to_string(imgCount) + ".png", image);
#ifdef STD_LOG
            std::cout << "save a Pic\n";
#endif
        }
        if(time_q.size() == 5) {
            fps = 1.0 / ((getTickCount() - time_q.front()) / getTickFrequency() / 5);
            //cout << "Fps " << fps << std::endl;
            time_q.pop();
        }
        time_q.push(getTickCount());
    }
}
